#include "drv_OpticalCar_TOF.h"


#define VL53L0X_IIC_ADDR  0x52;


static SoftIIC_HardwareTypeDef g_iic_hw_1, g_iic_hw_2;
static VL53L0X_Dev_t g_tof_1, g_tof_2;


static void VL53L0X_HardwareInit(void) {
    // ===== Config IIC hardware =====
    g_iic_hw_1.RCC_SCL = RCC_SCL1;
    g_iic_hw_1.RCC_SDA = RCC_SDA1;
    g_iic_hw_1.port_SCL = PORT_IIC_SCL1;
    g_iic_hw_1.pin_SCL = PIN_IIC_SCL1;
    g_iic_hw_1.port_SDA = PORT_IIC_SDA1;
    g_iic_hw_1.pin_SDA = PIN_IIC_SDA1;
    
    SoftIIC_Init(&g_iic_hw_1);
    
    g_iic_hw_2.RCC_SCL = RCC_SCL2;
    g_iic_hw_2.RCC_SDA = RCC_SDA2;
    g_iic_hw_2.port_SCL = PORT_IIC_SCL2;
    g_iic_hw_2.pin_SCL = PIN_IIC_SCL2;
    g_iic_hw_2.port_SDA = PORT_IIC_SDA2;
    g_iic_hw_2.pin_SDA = PIN_IIC_SDA2;
    
    SoftIIC_Init(&g_iic_hw_2);
    
    g_tof_1.iic_addr = 0x52;
    g_tof_2.iic_addr = 0x52;
    g_tof_1.iic_hw = &g_iic_hw_1;
    g_tof_2.iic_hw = &g_iic_hw_2;
}


void Car_TOF_Init(void) {
    VL53L0X_Error _err1, _err2;
    uint32_t refSpadCount;
    uint8_t isApertureSpads;
    uint8_t VhvSettings;
    uint8_t PhaseCal;
    
    // ===== I2C hardware init =====
    VL53L0X_HardwareInit();
    
    // ===== device init =====
    _err1 = VL53L0X_DataInit(&g_tof_1);
    _err2 = VL53L0X_DataInit(&g_tof_2);
    if (VL53L0X_ERROR_NONE != _err1 || VL53L0X_ERROR_NONE != _err2) {
        printf("ToF data init error: %i, %i", _err1, _err2);
        while (1);
    }
    
    _err1 = VL53L0X_StaticInit(&g_tof_1);
    _err2 = VL53L0X_StaticInit(&g_tof_2);
    if (VL53L0X_ERROR_NONE != _err1 || VL53L0X_ERROR_NONE != _err2) {
        printf("ToF static init error: %i, %i", _err1, _err2);
        while (1);
    }
    
    // ===== calibration =====
    _err1 = VL53L0X_PerformRefCalibration(&g_tof_1, &VhvSettings, &PhaseCal); // Device Initialization
    _err2 = VL53L0X_PerformRefCalibration(&g_tof_2, &VhvSettings, &PhaseCal);
    if (VL53L0X_ERROR_NONE != _err1 || VL53L0X_ERROR_NONE != _err2) {
        printf("Ref Calibration error: %i, %i", _err1, _err2);
        while (1);
    }
    
    _err1 = VL53L0X_PerformRefSpadManagement(&g_tof_1, &refSpadCount, &isApertureSpads); // Device Initialization
    _err2 = VL53L0X_PerformRefSpadManagement(&g_tof_2, &refSpadCount, &isApertureSpads);
    if (VL53L0X_ERROR_NONE != _err1 || VL53L0X_ERROR_NONE != _err2) {
        printf("Pad Calibration error: %i, %i", _err1, _err2);
        while (1);
    }
    
    // ===== dev mode =====
    _err1 = VL53L0X_SetDeviceMode(&g_tof_1, VL53L0X_DEVICEMODE_SINGLE_RANGING);
    _err2 = VL53L0X_SetDeviceMode(&g_tof_2, VL53L0X_DEVICEMODE_SINGLE_RANGING);
    if (VL53L0X_ERROR_NONE != _err1 || VL53L0X_ERROR_NONE != _err2) {
        printf("ToF set mode error: %i, %i", _err1, _err2);
        while (1);
    }
    
    printf("Init Tof Succuss.");
}


VL53L0X_Error Car_TOF_Measure(VL53L0X_RangingMeasurementData_t *m1, VL53L0X_RangingMeasurementData_t *m2) {
    VL53L0X_Error _err1, _err2;
    
    _err1 = VL53L0X_PerformSingleRangingMeasurement(&g_tof_1, m1);
    _err2 = VL53L0X_PerformSingleRangingMeasurement(&g_tof_2, m2);
    
    if (VL53L0X_ERROR_NONE != _err1) {
        return _err1;
    }
    else if (VL53L0X_ERROR_NONE != _err2) {
        return _err2;
    }
    
    return _err1;
}
